/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/

#ifndef __AHRS_GLVIEWER_H__
#define __AHRS_GLVIEWER_H__

#include <QtCore>
#include <QtGui>

#include <QGLViewer/qglviewer.h>

#include "base/types/quaternions.h"
#include "gui/mesh_off.h"



class AHRS_GLViewer : public QGLViewer
{
public :
    AHRS_GLViewer(QWidget *parent);

    ///
    /// \brief Load a 3D model mesh
    /// \param fn_mesh - mesh file name
    /// \return
    ///
    int load_mesh(std::string &fn_mesh);

    ///
    /// \brief Set Euler angles (yaw, pitch, roll)
    /// \param yaw_   - Yaw angle (in degree)
    /// \param pitch_ - Pitch angle (in degree)
    /// \param roll_  - Roll angle (in degree)
    /// \return
    ///
    int set_angles(double yaw_, double pitch_, double roll_) {
        m_yaw   = -yaw_ + 90.0;
        m_pitch = -pitch_;
        m_roll  = roll_;

        m_ang.fromEulerDeg(yaw_, pitch_, roll_);

        return 0;
    }

    ///
    /// \brief Set displacement
    ///
    /// \param disp - displacement (x,y,z)
    ///
    /// \return
    ///
    int set_disp(double *disp_) {
        m_disp[0] = disp_[0];
        m_disp[1] = disp_[1];
        m_disp[2] = disp_[2];
        return 0;
    }


protected:
    virtual void draw();
    virtual QString helpString() const;
    virtual void timerEvent(QTimerEvent *event);
    virtual void keyPressEvent(QKeyEvent *e);
    virtual void closeEvent(QCloseEvent *e);

    ///
    /// \brief Draw a model (airplane) for show current oriention
    ///
    void draw_1();

    ///
    /// \brief Draw axes to show current orientions
    ///
    void draw_2();

protected:
    MeshData                    m_mesh;
    pi::Quaternions<float>      m_ang;
    double                      m_yaw, m_pitch, m_roll;
    double                      m_disp[3];
};

#endif // end of __AHRS_GLVIEWER_H__
